using BEPUphysics.Collidables.MobileCollidables;
using BEPUphysics.CollisionShapes.ConvexShapes;
using BEPUphysics.Entities;
using Indiefreaks.Xna.Logic;
using Indiefreaks.Xna.Physics.Entities;
using Microsoft.Xna.Framework;
using SynapseGaming.LightingSystem.Collision;

namespace Indiefreaks.AceOnSteroids.Logic.Behaviors
{
    /// <summary>
    /// This behavior forces the attached entity to keep at the vertical 0f plane even when it collides
    /// </summary>
    public class PlaneConstraintBehavior : Behavior
    {
        private Matrix _cachedWorldTransform;

        public PlaneConstraintBehavior()
        {
            // this behavior commands will only need to be executed when the game entity Y position value is different than 0f
            AddCondition(() => Agent.ParentObject.World.M42 != 0f);

            // if that is the case, we'll force the game entity position to be aligned to the 0f Y plane
            AddLocalCommand(Force2DConstraint);
            // moreover, if the current game entity uses a dynamic physical entity, we'll reset its linear velocity Y force to 0f to avoid this behavior to execute once collided
            AddLocalCommand(() => Agent.ParentObject is ICollisionObject && ((ICollisionObject) Agent.ParentObject).CollisionMove is ConvexHullCollisionMove, CorrectLinearForce);
        }

        private object Force2DConstraint(Command command)
        {
            // the M42 field stores the World Translation Y value.
            _cachedWorldTransform = Agent.ParentObject.World;
            _cachedWorldTransform.M42 = 0f;
            Agent.ParentObject.World = _cachedWorldTransform;

            // since it's a local command only, we return null
            return null;
        }

        private object CorrectLinearForce(Command command)
        {
            Entity<ConvexCollidable<TransformableShape>> collisionEntity = ((ConvexHullCollisionMove) ((ICollisionObject) Agent.ParentObject).CollisionMove).Entity;
            collisionEntity.LinearVelocity = new Vector3(collisionEntity.LinearVelocity.X, 0f, collisionEntity.LinearVelocity.Z);

            return null;
        }
    }
}